#ifndef ARAL_INTERFACE_STATE_HPP
#define ARAL_INTERFACE_STATE_HPP

#include <string>
#include <vector>
#include <functional>
#include "aral/data_structure_definition.hpp"

namespace ARAL::interface {

//机器人状态管理
class State
{
public:
    virtual ~State() = default;

    /**
     * @brief 初始化机器人状态(根据机械臂的实际状态更新起始点)
     *        切换控制器(机械臂的运动由另外控制器控制)后需要调用本函数重新初始化
     * @param q: 关节起始位置, 单位rad
     * @param qd: 关节起始速度, 单位rad/s
     * @param qdd: 关节起始加速度, 单位rad/s^2
     * @param t_w: 当前运动对应的工具和工件信息
     * @return if < 0, 则表示设置失败
     */
    virtual int rsInitiateRobotState(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const ToolWorkpiece& t_w) = 0;

    /**
     * @brief 设置GMO外力估计增益。增益越大, 检测灵敏度提升, 但是会放大噪声。
     * @param gain: 具体的增益值, 目前支持的设置区间为[20, 150]
     * @return if < 0, 则表示设置失败
     */
    virtual int rsSetExternalForceEstimationGain(const interface::RLJntArray& gain) = 0;

    /**
     * @brief 更新机器人关节状态(如果用户不能给出运动学全部信息, 可以先调用 utlKalmanFilterPVA 函数进行估计)
     *        调用这个接口会连带更新机械臂所有连杆在笛卡尔空间的运动学信息, 包括位姿, 速度和加速度等
     *        调用这个接口会连带更新机械臂所有连杆的动力学信息, 包括动力学力矩、重力矩、外力矩等
     * @param q: 关节当前位置, 单位rad
     * @param qd: 关节当前速度, 单位rad/s
     * @param qdd: 关节当前加速度, 单位rad/s^2
     * @param torque: 关节实际力矩, 单位Nm
     * @param temperature: 关节温度, 单位℃
     * @param fricition: 关节摩擦力, 单位Nm
     * @param is_user_acc: 是否采用用户输入的加速度(默认是否)
     * @return if < 0, 则表示设置失败
     */
    virtual int rsUpdateJointState(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const RLJntArray& torque, const RLJntArray& temperature, const RLJntArray& friction, const bool& is_user_acc = false) = 0;

    /**
     * @brief 更新力传感器的信息, 处于力控模式时每个周期都需更新(传感器的偏置和量程限制需要在软件层处理)
     * (！！！在使用时先更新传感器信息, 再调用 rsUpdateJointCTF 更新机械臂实时状态信息)
     * @param type: END 或者 BASE 或者 JOINT
     * @param data: 传感器在每个维度方向的测量数据
     * @return if < 0, 则表示设置失败
     */
    virtual int rsUpdateFTSensorData(const FTSensorType& type, const double* data) = 0;

    /**
     * @brief 设置参考轨迹
     * @param has_ref_traj: 是否有参考轨迹: 1) false, 表示没有参数轨迹, 用于力控拖动的场景; TrajectoryPoint 参数无需设置
     *                                    2) true,  表示有参考轨迹(一般由规划算法 tpUpdateCycle 函数生成). 在外部轴/外部工具的同步控制应用中, 用户需要显式更新 TrajectoryPoint 结构体中的ToolWorkpiece属性
     * @param point: 一般是规划输出的轨迹点
     * @return if < 0, 则表示设置失败
     */
    virtual int rsSetReferenceTrajectory(const bool& has_ref_traj, const TrajectoryPoint& point = TrajectoryPoint()) = 0;

    /**
     * @brief 获得机械臂奇异属性(调用 rsUpdateJointPVA 更新机器人运动学后使用)
     * @return 奇异属性结构体
     */
    virtual const SingularProperty rsGetSingularityProperty() = 0;

    /**
     * @brief 设置关节最大速度(用于安全防护, 当实际速度超过给定的数值时会启动保护功能, 对机械臂运动轨迹进行调整)
     * @param speed: 角速度值, 单位rad/s
     * @return if < 0, 则表示设置失败
     */
    virtual int rsSetJointMaximumSpeed(const RLJntArray& speed) = 0;

    /**
     * @brief 设置末端在基坐标系下描述的最大速度(用于安全防护, 当实际速度超过给定的数值时会启动保护功能, 对机械臂运动轨迹进行调整)
     * (！！！如果为正常工具, 末端指的是工具; 如果为远端工具, 末端指的是工件)
     * @param speed: 线速度和角速度, 单位m/s和rad/s
     * @return if < 0, 则表示设置失败
     */
    virtual int rsSetEndMaximumSpeedInBase(const Array2d& speed) = 0;

    /**
     * @brief 获得当前末端在基坐标系下描述的位姿(调用 rsUpdateJointPVA 更新机器人运动学后使用)
     * (！！！如果为正常工具, 末端指的是工具; 如果为远端工具, 末端指的是工件)
     * @return 各个方向的位置和姿态
     */
    virtual const RLPose rsGetEndPoseInBase() const = 0;

    /**
     * @brief 获得当前末端在基坐标系下描述的外力(调用 rsUpdateJointPVA 更新机器人运动学，以及调用 rsUpdateJointCTF 更新机器人动力学后使用)
     * (！！！如果为正常工具, 末端指的是工具; 如果为远端工具, 末端指的是工件)
     * @return 各个方向的力和力矩
     */
    virtual const RLWrench rsGetEndExternalWrenchInBase() const = 0;

    /**
     * @brief 获得当前末端在末端坐标系下描述的外力(调用 rsUpdateJointPVA 更新机器人运动学，以及调用 rsUpdateJointCTF 更新机器人动力学后使用)
     * (！！！如果为正常工具, 末端指的是工具; 如果为远端工具, 末端指的是工件)
     * @return 各个方向的力和力矩
     */
    virtual const RLWrench rsGetEndExternalWrenchInEnd() const = 0;

    /**
     * @brief 获得当前关节外力矩(调用 rsUpdateJointPVA 更新机器人运动学，以及调用 rsUpdateJointCTF 更新机器人动力学后使用)
     * @return 外力矩值
     */
    virtual const RLJntArray rsGetJointExternalTorque() const = 0;

    /**
     * @brief 获得当前关节动力学力矩(调用 rsUpdateJointPVA 更新机器人运动学，以及调用 rsUpdateJointCTF 更新机器人动力学后使用)
     * @return 动力学力矩值
     */
    virtual const RLJntArray rsGetJointDynamicTorque() const = 0;

    /**
     * @brief 获得当前关节重力矩(调用 rsUpdateJointPVA 更新机器人运动学，以及调用 rsUpdateJointCTF 更新机器人动力学后使用)
     * @return 重力矩值
     */
    virtual const RLJntArray rsGetJointGravityTorque() const = 0;

};//class State 
typedef std::shared_ptr<State>  StatePtr;

}   //namespace ARAL::interface

#endif // ARAL_INTERFACE_STATE_HPP